#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float64MultiArray.h>
#include <iostream>
#include <fstream>
// #include <cstring>
#include "trapezoidal_spd.cpp"

const int NUM_JOINT = 6;
const double PA_RATIO[] = {30, 205.0 / 3, 50, 125.0 / 2, 125.0 / 2, 200 / 9};
const double PI = 3.1415926535;

// interpolation: using parameters of septic interpolation to compute the velocity
// input: a - parameters of the interpolation curve
// 		  t - current time
// output: res - the velocity of the current time
double interpolation(double a[8], double t)
{
    double res = 0;
    for (int i = 7; i >= 1; i--)
    {
        res = res * t + a[i] * i;
    }
    return res;
}

// Plan: use theta plan the first trapezoidal curve, and use interpolation parameters to plan the reset
// input: theta_start(array) - the start angle of all joints
//        theta_end(array) - the end angle of all joints
//        a(array) - the eight interpolation parameters for all joints
// output: None
void Plan(double theta_start[], double theta_end[], ros::Publisher &pos_pub, double a[6][8])
{

    // std_msgs::Float32MultiArray msg;
    std_msgs::Float64MultiArray msg;
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);

    InParam in_param[NUM_JOINT];
    int sign[NUM_JOINT];
    CurveParam curve[NUM_JOINT];
    // double sum_dis = 0;
    double sum_dis[NUM_JOINT] = {0};

    // define the upper bound for the vel and acc
    double aa_max[NUM_JOINT] = {1, 1, 1, 1, 1, 1};
    double acc_max[NUM_JOINT] = {0.3491, 0.2618, 0.3491, 0.4363, 0.4363, 0.4363};
    double vel_max[NUM_JOINT] = {0.7854, 0.6981, 0.7854, 0.9599, 0.9599, 0.9599};

    for (int i = 0; i < NUM_JOINT; i++)
    {
        in_param[i].aa_max = aa_max[i];
        in_param[i].acc_max = acc_max[i];
        in_param[i].vel_max = vel_max[i];
        // if start <>> end, let start = 0, end = end - start, rotate in positive direction
        if (theta_start[i] < theta_end[i])
        {
            sign[i] = 1;
            in_param[i].theta_start = 0.0;
            in_param[i].theta_end = theta_end[i] - theta_start[i];
        }
        // if start > end, let start = 0, end = start - end, rotate in negative direction
        else
        {
            sign[i] = -1;
            in_param[i].theta_start = 0.0;
            in_param[i].theta_end = theta_start[i] - theta_end[i];
        }

        sum_dis[i] = theta_start[i];

        Trapezoidal(in_param[i], curve[i]);
    }

    int frequent = 100; //frequent
    ros::Rate loop_rate(frequent);
    double step = 1.0 / frequent;
    double tolerance = 1e-10;
    double start_stop = 0.5; // when to start judging the stop condition (velocity = 0)

    bool switch_plan = false; // a flag to judge when to switch from trapezoidal to interpolation

    // for test, if you want to check the output, please change the file_name to definite path
    // ofstream ofile;
    // string file_name = "~/ROS/probot_ws/output/out.txt";
    // ofile.open(file_name);

    int cnt_loop = 0;

    for (double t = 0;; t += step)
    {
        double sum_w = 0; // the sum of velocity of all joints
        double w = 0;     // velocity

        std::cout << "time: " << t << " start_stop: " << start_stop << std::endl;
        // ofile << "time: " << t << " start_stop: " << start_stop << std::endl;        // for test

        for (int i = 0; i < NUM_JOINT; i++)
        {
            if (!switch_plan)
            {
                w = sign[i] * compute(t, curve[i]); // obtain the rotation velocity from the curve
            }
            else
            {
                w = interpolation(a[i], t); // obtain velocity from interpolation
            }

            if (fabs(w) < tolerance)
            {
                w = 0.0;    // when velocity is too small, set it to zero
            }

            sum_w += fabs(w); // calculate the sum velocity of all joints
            sum_dis[i] += w * step;     // calculate the rotation angle

            // msg.data.at(i) = sign[i] * w * PA_RATIO[i] / PI * 180;		// in real probot
            msg.data.at(i) =  w; // in gazebo
            std::cout << i << " speed:" << w << " dist:" << sum_dis[i] << std::endl;
            // ofile << i << " speed:" << w << " dist:" << sum_dis[i] << std::endl;     // for test
        }

        std::cout << "sum speed:" << sum_w << std::endl;
        // ofile << "sum speed:" << sum_w << std::endl;     // for test

        // when all veloctiy is close to zero, and has started for a while, then it need to stop
        if (sum_w < NUM_JOINT * tolerance && t >= start_stop && !switch_plan)
        {
            switch_plan = true;
            t = 0 - step;
            // ofile << "########## Switch ##########" << std::endl;        // for test
            std::cout << "##############################" << std::endl;
            std::cout << "########## Switch ##########" << std::endl;
            std::cout << "##############################" << std::endl;
        }
        // when a loop of interpolation curve finished, reset time, 这里的11是七次插值的t3，需要调完之后改
        if (switch_plan && fabs(t - 11.0) <= tolerance)
        {
            t = 0 - step;
            cnt_loop++;
            // ofile << "********** Loop **********" << std::endl;      // for test
            std::cout << "******************************" << std::endl;
            std::cout << "********** Loop **********" << std::endl;
            std::cout << "******************************" << std::endl;
        }
        // if (switch_plan && fabs(t - 4.0) <= tolerance)      // for test
        // {
        //     msg.data.at(0) = 0.0;
        //     msg.data.at(1) = 0.0;
        //     msg.data.at(2) = 0.0;
        //     msg.data.at(3) = 0.0;
        //     msg.data.at(4) = 0.0;
        //     msg.data.at(5) = 0.0;
        //     pos_pub.publish(msg);
        //     break;
        // }
        
        pos_pub.publish(msg);
        
        // for test, the interpolation loop for 3 times
        if (cnt_loop == 3)
        {
            break;
        }

        loop_rate.sleep();
    }
    ofile.close();
}

int main(int argc, char **argv)
{
    // Actually, you can simply use the following command in terminal:
    // rostopic pub /probot_anno/arm_pos_controller/command std_msgs/Float64MultiArray "data: [0.5232, -0.8132, -0.1175, 0, 0.9308, 0]"
    // rostopic pub /probot_anno/arm_pos_controller/command std_msgs/Float64MultiArray "data: [0, -0.7404, 0.312, 0, 0.4284077, 0]"
    // rostopic pub /probot_anno/arm_pos_controller/command std_msgs/Float64MultiArray "data: [-0.7086, -1.447, 0.5653, 0, 0.8820, 0]"
    // rostopic pub /probot_anno/arm_pos_controller/command std_msgs/Float64MultiArray "data: [0, 0, 0, 0, 0, 0]"

    // Here is an example to use C++.
    // Define a standard message and add 6 elements in it.
    double PI = 3.1415926535;
    // std_msgs::Float32MultiArray msg;
    std_msgs::Float64MultiArray msg;
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);
    msg.data.push_back(0);
    // msg.data.push_back(0);

    // Initialize the node.
    ros::init(argc, argv, "control_example");

    // Create a handle to the processing node.
    ros::NodeHandle n;

    // Initialize a publisher object.
    ros::Publisher pos_pub = n.advertise<std_msgs::Float64MultiArray>("/probot_anno/arm_vel_controller/command", 100);      // for gazebo
    // ros::Publisher pos_pub = n.advertise<std_msgs::Float32MultiArray>("speed_chatter", 1000);        // for real

    // Specify a frequency that you would like to loop at.
    ros::Rate loop_rate(100);

    double theta0[] = {0, 0, 0, 0, 0, 0};
    // the rotation angle of each joint at three points
    double theta1[] = {0.523, -0.813, -0.117, 0, 0.931, 0};
    double theta2[] = {0, -0.7404, 0.312, 0, 0.4284077, 0};
    double theta3[] = {-0.7086, -0.8133, -0.1175, 0, 0.9308, 0};
    
    // paratmeters of interpolation curve !!要很精确，直接从matlab的数据里copy，保留matlab能保留的最大位数
    double a[6][8] = {{0.523278320000027, -1.35346922754327e-14, 9.74049002337209e-16, 0.0255599479124147,
                       -0.0212927723962884, 0.00453968431076886, -0.000374291439352514, 1.07602385507804e-05},
                      {-0.813276739999986, -2.39994547342673e-14, 2.88228831079673e-15, 0.0615646321404294,
                       -0.0310772824716050, 0.00542283081587105, -0.000400475780946317, 1.07339806547540e-05},
                      {-0.117538640000014, 6.31466410374825e-16, 4.70988506451852e-16, 0.00556550163933514,
                       0.00360206258898156, -0.00125835599398683, 0.000122758886965499, -3.84667668853361e-06},
                      {0},
                      {0.930815380000000, 2.33679883238924e-14, -3.35327681724858e-15, -0.0671301337797645,
                       0.0274752198826235, -0.00416447482188422, 0.000277716893980818, -6.88730396622037e-06},
                      {0}};
    // for test
    // double a[6][8] = {{0},
    //                   {0},
    //                   {0},
    //                   {0},
    //                   {0.930815380000000, 2.33679883238924e-14, -3.35327681724858e-15, -0.0671301337797645,
    //                     0.0274752198826235, -0.00416447482188422, 0.000277716893980818, -6.88730396622037e-06},
    //                   {0}};

    Plan(theta0, theta1, pos_pub, a);

    // while (1)
    // {
    // 	msg.data.at(0) = 1.0;
    // 	pos_pub.publish(msg);
    // 	loop_rate.sleep();
    // }

    return 0;
}
